/*
 * <one line to give the program's name and a brief idea of what it does.>
 * Copyright (C) 2016  <copyright holder> <email>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#ifndef POINTCLOUDMAPPING_H
#define POINTCLOUDMAPPING_H

#include "System.h"

#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>
#include <condition_variable>



using namespace ORB_SLAM2;



class PointCloudMapping
{
public:
    typedef pcl::PointXYZRGBA PointT;
    typedef pcl::PointCloud<PointT> PointCloud;

    PointCloudMapping( double resolution_ );

    // 插入一个keyframe，会更新一次地图
    void InsertKeyFrame(KeyFrame* kf);
    void Run();
    void public_cloud(pcl::PointCloud<pcl::PointXYZRGBA> &cloud_out);
    void Cloud_transform(PointCloud::Ptr &source, PointCloud::Ptr &out);

    bool isStopped();
    void RequestStop();
    void RequestStart();
    void Release();
    void RequestShutDown();
    void Save();

private:

    bool CheckNewKeyFrame();
    void JointNewKeyFrame();
    void UpdateShow();


    bool Stop();
    bool CheckShutDown();
    void SetShutDown();

protected:
    PointCloud::Ptr generatePointCloud(KeyFrame* kf, cv::Mat& color, cv::Mat& depth);

    static pcl::visualization::CloudViewer mViewer;

    PointCloud mPointCloud;
    PointCloud::Ptr mpGlobalMap;
    shared_ptr<thread>  mViewerThread;

    //condition_variable  keyFrameUpdated;
    std::mutex               keyFrameUpdateMutex;

    // data to generate point clouds
    vector<KeyFrame*>       mKeyFrames;
    vector<cv::Mat>         mColorImgs;
    vector<cv::Mat>         mDepthImgs;
    list<uint64_t>          mNewKeyFrame;
    std::mutex                   mKeyFrameMutex;

    double resolution = 0.01;
    pcl::VoxelGrid<PointT>  voxel;
    pcl::StatisticalOutlierRemoval<PointT> sor;// 创建滤波器对象
    pcl::PassThrough<PointT> pass;

    bool mbStopped;
    bool mbStopRequested;
    bool mbShutDownRequested;
    bool mShutDownFlag;
    std::mutex mMutexStop;
    std::mutex mMutexGlobalMap;
    std::mutex mMutexShutDown;
/*
public:
	octomap::ColorOcTree octo_tree(0.01);
*/
};





#endif // POINTCLOUDMAPPING_H
